#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"
#include <rclcpp/rclcpp.hpp>

using geometry_msgs::msg::Twist;
using namespace std::chrono_literals;

class TfListenerNode : public rclcpp::Node {
public:
  TfListenerNode() : Node("tf_listener_node") {

    RCLCPP_INFO(this->get_logger(), "初始化动态监听器节点！");

    // 父坐标系
    this->declare_parameter<std::string>("target_frame", "turtle2");
    target_frame = this->get_parameter("target_frame").as_string();
    // 子坐标系
    this->declare_parameter<std::string>("source_frame", "turtle1");
    source_frame = this->get_parameter("source_frame").as_string();

    // 创建 tf2 缓冲区对象
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    // 创建动态监听器对象
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // 速度发布方
    cmd_vel =
        this->create_publisher<Twist>("/" + target_frame + "/cmd_vel", 10);

    // 创建定时器，周期性获取坐标变换
    timer_ =
        this->create_wall_timer(1s, std::bind(&TfListenerNode::on_timer, this));
  }

private:
  // 父坐标系
  std::string target_frame;
  // 子坐标系
  std::string source_frame;

  // 速度发布
  rclcpp::Publisher<Twist>::SharedPtr cmd_vel;

  // tf2 缓冲区对象的智能指针
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  // 动态监听器对象的智能指针
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  // 定时器
  rclcpp::TimerBase::SharedPtr timer_;
  // 定时器回调函数
  void on_timer() {
    // 获取坐标变换
    geometry_msgs::msg::TransformStamped ts;
    try {
      // 目标坐标系
      // 源坐标系
      // 获取最新的坐标变换
      ts = tf_buffer_->lookupTransform(target_frame, source_frame,
                                       tf2::TimePointZero);

      auto twist = std::make_shared<Twist>();
      twist->linear.set__x(0.5 * sqrt(pow(ts.transform.translation.x, 2) +
                                      pow(ts.transform.translation.y, 2)));
      twist->angular.set__z(1.0 * std::atan2(ts.transform.translation.y,
                                             ts.transform.translation.x));
      cmd_vel->publish(*twist);

    } catch (const tf2::TransformException &ex) {
      RCLCPP_WARN(this->get_logger(), "Could not transform: %s", ex.what());
      return;
    }
    // 输出坐标变换
    RCLCPP_INFO(this->get_logger(), "获取坐标变换成功！");
    RCLCPP_INFO(this->get_logger(), "目标坐标: %s", ts.header.frame_id.c_str());
    RCLCPP_INFO(this->get_logger(), "原坐标: %s", ts.child_frame_id.c_str());
    RCLCPP_INFO(this->get_logger(), "平移: [%.2f, %.2f, %.2f]",
                ts.transform.translation.x, ts.transform.translation.y,
                ts.transform.translation.z);
    RCLCPP_INFO(this->get_logger(), "旋转: [%.2f, %.2f, %.2f, %.2f]",
                ts.transform.rotation.x, ts.transform.rotation.y,
                ts.transform.rotation.z, ts.transform.rotation.w);
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TfListenerNode>());
  rclcpp::shutdown();
  return 0;
}
